#include "Surface.h"
#include <cmath>
#include <opencv2\core\core.hpp>
#include <iostream>
#include <queue>

#define _USE_MATH_DEFINES
#include <math.h>



Surface::Surface(int id) : surfaceID(id), boundary(), boundary_temp(), planeApproximation(), 
basis_x(0), basis_y(0), nx(0.0), ny(0.0), nz(0.0), d(0.0), x_boundary(0), y_boundary(0){
	

}

Surface::~Surface(){
	boundary.~fibonacci_heap();
	boundary_temp.~Mat_();
	planeApproximation.~Mat_();
}


void Surface::compressSurface(){
	storeBoundary();
	getPlaneParameters();
}

void Surface::accumulateBoundary(cv::Mat_<float> point){
	boundary_temp.push_back(point);
}


void Surface::accumulatePoints(float x, float y, float z, bool is_boundary){

	//row matrix
	cv::Mat_<float> in(1, 3);

	//m to mm
	in.at<float>(0, 0) = x * 1000.0;
	in.at<float>(0, 1) = y * 1000.0;
	in.at<float>(0, 2) = z * 1000.0;

	planeApproximation.push_back(in);

	if(is_boundary)
		accumulateBoundary(in);
	
}

void Surface::setRelativeCoordinateSystem(){
	switch(p_face){
	case PLANE_DIRECTION::X:
		x_boundary = 1;
		y_boundary = 2;
		//std::cout << "Y-Z plane" << std::endl;
		break;

	case PLANE_DIRECTION::Y:
		x_boundary = 0;
		y_boundary = 2;
		//std::cout << "X-Z plane" << std::endl;
		break;

	case PLANE_DIRECTION::Z:
		x_boundary = 0;
		y_boundary = 1;
		//std::cout << "X-Y plane" << std::endl;
		break;

	default:
		//error
		std::cout << "Plane direction not determined" << std::endl;
		x_boundary = 0;
		y_boundary = 0;
		return;
		break;
	}
}

void Surface::setBasis(){
	setRelativeCoordinateSystem();
	basis_x = plane_center.at<float>(0, x_boundary);
	basis_y = plane_center.at<float>(0, y_boundary);
}


void Surface::storeBoundary(){
	setBasis();
	size = getBoundarySize();

	cv::Vec2d b;
	for(int i = 0; i < size; i += std::max(size / 1000, 5)){
		b.val[0] = boundary_temp.at<float>(i, x_boundary);
		b.val[1] = boundary_temp.at<float>(i, y_boundary);
		boundary.push(getPolarCoordinate(b));
	}
}



Surface::polarCoordinate Surface::getPolarCoordinate(double x, double y) const{
	x -= basis_x;
	y -= basis_y;

	float r = (float) sqrt(x * x + y * y);
	float theta = atan2((long double) y, (long double) x);

	
	return polarCoordinate(r, theta);	
}


Surface::polarCoordinate Surface::getPolarCoordinate(cv::Vec2d in) const{
	return getPolarCoordinate(in.val[0], in.val[1]);
}


int Surface::getBoundarySize() const {
	return boundary_temp.rows;
}

int Surface::getPointsSize() const {
	return planeApproximation.rows;
}

void Surface::getPlaneParameters(){
	cv::PCA planeParam(planeApproximation, cv::Mat(), CV_PCA_DATA_AS_ROW);

	//normal as the eigenvector with smallest eigenvalue.
	nx = planeParam.eigenvectors.at<float>(2, 0);
	ny = planeParam.eigenvectors.at<float>(2, 1);
	nz = planeParam.eigenvectors.at<float>(2, 2);

	//normalize
	float norm = sqrt(nx*nx + ny*ny + nz*nz);
	nx /= norm;
	ny /= norm;
	nz /= norm;

	
	//Distance from origin
	plane_center = planeParam.mean;
	d = nx * plane_center.at<float>(0, 0) + ny * plane_center.at<float>(0, 1) + nz * plane_center.at<float>(0, 2);
	
	//std::cout << "dimension variance: " <<	planeParam.eigenvalues << std::endl;
	//std::cout << nx << "\t" << ny << "\t" << nz << "\t" << d << std::endl;
	//std::cout << "=============================" << std::endl;


	//Determine which direction to disregard
	determineDominentFactor();
}

void Surface::determineDominentFactor(){
	if(abs(nx) > abs(ny) && abs(nx) > abs(nz)){
		p_face = PLANE_DIRECTION::X;
	} else if (abs(ny) > abs(nx) && abs(ny) > abs(nz)){
		p_face = PLANE_DIRECTION::Y;
	} else {
		p_face = PLANE_DIRECTION::Z;
	}
}


bool Surface::comparePlaneParameters(Surface* other) const{
	//Check locality, normal vector directions
	double angle = acos(sqrt(nx * other->nx + ny * other->ny + nz * other->nz));
	double dist_origin = abs(d - other->d);
	double dist_min = (plane_center - other->plane_center).dot(plane_center - other->plane_center);
	
	return angle < M_PI / 8.0 && dist_origin < 20.0 && dist_min < 500.0;	
}


int Surface::getSurfaceID() const{
	return surfaceID;
}


void Surface::mergeSurface(Surface* other){


}


void Surface::changePole(cv::Mat_<float> new_pole){
	std::queue<cv::Vec2d> temp = std::queue<cv::Vec2d>();

	//Empty the heap and store the contents to temporary storage
	while(!boundary.empty()){
		temp.push(getCartesianCoordinate(boundary.top()));
		boundary.pop();
	}

	//Set the new pole
	plane_center = new_pole;
	setBasis();
	
	//Fill the heap with modified polar coordinates
	while(!temp.empty()){
		boundary.push(getPolarCoordinate(temp.front()));
		temp.pop();
	}
}

cv::Vec2d Surface::getCartesianCoordinate(Surface::polarCoordinate in) const{
	cv::Vec2d out;
	out.val[0] = cos(in.theta) + basis_x;
	out.val[1] = sin(in.theta) + basis_y;
	return out;
}

cv::Vec3d Surface::get3DCartesianCoordinate(polarCoordinate in) const {
	cv::Vec2d xy = getCartesianCoordinate(in);
	cv::Vec3d out;

	out.val[x_boundary] = xy.val[0];
	out.val[y_boundary] = xy.val[1];

	//ax + by + cz = d
	if(p_face == PLANE_DIRECTION::X){
		out.val[0] = (d - out.val[1] * ny - out.val[2] * nz) / nx;
	} else if(p_face == PLANE_DIRECTION::Y){
		out.val[1] = (d - out.val[0] * nx - out.val[2] * nz) / ny;
	} else {
		out.val[2] = (d - out.val[0] * nx - out.val[1] * ny) / nz;
	}

	return out;
}

std::vector<cv::Vec3d> Surface::getReconstructedPointsVector() const {
	std::vector<cv::Vec3d> out;
	boost::heap::fibonacci_heap<polarCoordinate, boost::heap::compare<comparePolarCoordinate>> boundary_cp(boundary);

	while(!boundary_cp.empty()){
		out.push_back(get3DCartesianCoordinate(boundary_cp.top()));
		boundary_cp.pop();
	}

	return out;
}



bool Surface::isCenterOnPlane(){
	return false;

}